#include <iostream>
#include <string>
#include <deque>
#include <thread>
#include <Eigen/Eigen>
#include <Eigen/Core>

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#include <unitree_legged_msgs/HighState.h>
#include "X20_LegKinematic.hpp"

class JointMsg2A1Msg
{
public:
    std::string root_dir = ROOT_DIR;
    std::string newBagFile_dir;
    std::string outBagFiles;

    ros::NodeHandle nh;
    ros::Subscriber subLegOdom;
    ros::Subscriber subLidar;
    ros::Subscriber subImu;
    ros::Subscriber subJointState;

    rosbag::Bag newBag;

    std::string A1_leg_odom_Topic = "/state_SDK";
    std::string A1_lidar_Topic = "/points_raw";
    std::string A1_imu_Topic = "/imu_raw";
    std::string A1_jointState_Topic = "/high_state";
    
    std::string leg_odom_Topic = "/leg_odom";
    std::string lidar_Topic = "/velodyne_points";
    std::string imu_Topic = "/xsens/imu/data";
    std::string jointState_Topic = "/joint_states";

    X20LegKinematic X20Leg;
    sensor_msgs::Imu imu_data;
    double roll, pitch, yaw;

    JointMsg2A1Msg()
    {
        YAML::Node yaml;
        yaml = YAML::LoadFile(std::string(std::string(ROOT_DIR)) + "config/JointMsg2A1MsgConfig.yaml");
        newBagFile_dir = yaml["newBagFile_dir"].as<std::string>();
        outBagFiles = std::string(std::string(ROOT_DIR)) + newBagFile_dir;
        int reflash = system((std::string("exec rm ") + outBagFiles).c_str());

        subLegOdom    = nh.subscribe<nav_msgs::Odometry>(leg_odom_Topic, 1000, &JointMsg2A1Msg::legOdometry_handle, this, ros::TransportHints().tcpNoDelay());
        subLidar      = nh.subscribe<sensor_msgs::PointCloud2>(lidar_Topic, 1000, &JointMsg2A1Msg::lidar_handle, this, ros::TransportHints().tcpNoDelay());
        subImu        = nh.subscribe<sensor_msgs::Imu>(imu_Topic, 1000, &JointMsg2A1Msg::imu_handle, this, ros::TransportHints().tcpNoDelay());
        subJointState = nh.subscribe<sensor_msgs::JointState>(jointState_Topic, 1000, &JointMsg2A1Msg::jointState_handle, this,  ros::TransportHints().tcpNoDelay());


        newBag.open(outBagFiles, rosbag::bagmode::Write);
    }


    /***********************************************************
     * @brief X20关节数据话题的接收回调函数
     * 
     * @param msgIn 
     **********************************************************/
    void jointState_handle(const sensor_msgs::JointState::ConstPtr& msgIn)
    {
        X20Leg.getX20InfoFromMsg(msgIn);

        sensor_msgs::JointState joint_data = *msgIn;

        unitree_legged_msgs::HighState data;
        data.stamp = joint_data.header.stamp;

        // joint
        data.motorState[0].q = X20Leg.foot_angle(0, 1);
        data.motorState[0].dq = X20Leg.foot_angle_vel(0, 1);
        data.motorState[0].tauEst = X20Leg.foot_torque(0, 1);
        data.motorState[1].q = X20Leg.foot_angle(1, 1);
        data.motorState[1].dq = X20Leg.foot_angle_vel(1, 1);
        data.motorState[1].tauEst = X20Leg.foot_torque(1, 1);
        data.motorState[2].q = X20Leg.foot_angle(2, 1);
        data.motorState[2].dq = X20Leg.foot_angle_vel(2, 1);
        data.motorState[2].tauEst = X20Leg.foot_torque(2, 1);

        data.motorState[3].q = X20Leg.foot_angle(0, 0);
        data.motorState[3].dq = X20Leg.foot_angle_vel(0, 0);
        data.motorState[3].tauEst = X20Leg.foot_torque(0, 0);
        data.motorState[4].q = X20Leg.foot_angle(1, 0);
        data.motorState[4].dq = X20Leg.foot_angle_vel(1, 0);
        data.motorState[4].tauEst = X20Leg.foot_torque(1, 0);
        data.motorState[5].q = X20Leg.foot_angle(2, 0);
        data.motorState[5].dq = X20Leg.foot_angle_vel(2, 0);
        data.motorState[5].tauEst = X20Leg.foot_torque(2, 0);

        data.motorState[6].q = X20Leg.foot_angle(0, 3);
        data.motorState[6].dq = X20Leg.foot_angle_vel(0, 3);
        data.motorState[6].tauEst = X20Leg.foot_torque(0, 3);
        data.motorState[7].q = X20Leg.foot_angle(1, 3);
        data.motorState[7].dq = X20Leg.foot_angle_vel(1, 3);
        data.motorState[7].tauEst = X20Leg.foot_torque(1, 3);
        data.motorState[8].q = X20Leg.foot_angle(2, 3);
        data.motorState[8].dq = X20Leg.foot_angle_vel(2, 3);
        data.motorState[8].tauEst = X20Leg.foot_torque(2, 3);

        data.motorState[9].q = X20Leg.foot_angle(0, 2);
        data.motorState[9].dq = X20Leg.foot_angle_vel(0, 2);
        data.motorState[9].tauEst = X20Leg.foot_torque(0, 2);
        data.motorState[10].q = X20Leg.foot_angle(1, 2);
        data.motorState[10].dq = X20Leg.foot_angle_vel(1, 2);
        data.motorState[10].tauEst = X20Leg.foot_torque(1, 2);
        data.motorState[11].q = X20Leg.foot_angle(2, 2);
        data.motorState[11].dq = X20Leg.foot_angle_vel(2, 2);
        data.motorState[11].tauEst = X20Leg.foot_torque(2, 2);

        // foot_force
        Eigen::Matrix<double, 1, 4> foot_contact;
        foot_contact(0) = X20Leg.foot_contact(1);
        foot_contact(1) = X20Leg.foot_contact(0);
        foot_contact(2) = X20Leg.foot_contact(3);
        foot_contact(3) = X20Leg.foot_contact(2);

        for (int i = 0; i < 4; i++)
        {
            if(foot_contact(i) > 0)
            { data.footForce[i] = 250; }
            else
            { data.footForce[i] = 50; }
        }
       
        // imu
        data.imu.gyroscope[0] = imu_data.angular_velocity.x;
        data.imu.gyroscope[1] = imu_data.angular_velocity.y;
        data.imu.gyroscope[2] = imu_data.angular_velocity.z;
        data.imu.accelerometer[0] = imu_data.linear_acceleration.x;
        data.imu.accelerometer[1] = imu_data.linear_acceleration.y;
        data.imu.accelerometer[2] = imu_data.linear_acceleration.z;
        data.imu.quaternion[0] = imu_data.orientation.x;
        data.imu.quaternion[1] = imu_data.orientation.y;
        data.imu.quaternion[2] = imu_data.orientation.z;
        data.imu.quaternion[3] = imu_data.orientation.w;

        // imu-rpy
        data.imu.rpy[0] = roll;
        data.imu.rpy[1] = pitch;
        data.imu.rpy[2] = yaw;

        // other msgInfo
        data.position[0] = 0;
        data.position[1] = 0;
        data.position[2] = 0;
        
        data.velocity[0] = 0;
        data.velocity[1] = 0;
        data.velocity[2] = 0;


        newBag.write(A1_jointState_Topic, data.stamp, data);
    }




    /**************************************************************
     * @brief 订阅A1_legodom的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void legOdometry_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        nav_msgs::Odometry data = *msgIn;
        // data.header.stamp = ros::Time::now();
        data.header.frame_id = "camera_init";

        newBag.write(A1_leg_odom_Topic, data.header.stamp, data);   
    }


    /**************************************************************
     * @brief 订阅lidar的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void lidar_handle(const sensor_msgs::PointCloud2::ConstPtr& msgIn)
    {
        sensor_msgs::PointCloud2 data = *msgIn;
        // data.header.stamp = ros::Time::now();

        newBag.write(A1_lidar_Topic, data.header.stamp, data);
    }


    /**************************************************************
     * @brief 订阅imu的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void imu_handle(const sensor_msgs::Imu::ConstPtr& msgIn)
    {
        sensor_msgs::Imu data = *msgIn;
        // data.header.stamp = ros::Time::now();

        // Current IMU Info
        imu_data.angular_velocity = data.angular_velocity;
        imu_data.linear_acceleration = data.linear_acceleration;
        imu_data.orientation = data.orientation;

        tf2::Quaternion quat;
        tf2::fromMsg(data.orientation, quat);
        tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
        
        // Write to Bag
        newBag.write(A1_imu_Topic, data.header.stamp, data);
    }

    void processTopicsInfo()
    {
        ros::Rate rate(5000);
        while (ros::ok())
        {
            rate.sleep();
        }
    }

};


int main(int argc, char** argv)
{ 
    ros::init(argc, argv, "MsgProcess");

    JointMsg2A1Msg MO;

    ROS_INFO("\033[1;32m----> JointMsg2A1Msg Procee Started.\033[0m");

    std::thread MsgProcessThread(&JointMsg2A1Msg::processTopicsInfo, &MO);

    ros::spin();

    MsgProcessThread.join();

    return 0;
}